//
// Created by sun on 2020/10/20.
//

#ifndef SRC_ROSUTILS_H
#define SRC_ROSUTILS_H

#include <ros/ros.h>

class RosUtils{
public :


    template<typename T>
    static T readParam(ros::NodeHandle &n, std::string name);
    template<typename T>
    static T readParam(ros::NodeHandle &n, std::string name, T default_value);
};

template<typename T>
T RosUtils::readParam(ros::NodeHandle &n, std::string name) {
    T ans;
    name = ros::this_node::getName() + "/" + name;
    if (n.getParam(name, ans)) {
        ROS_INFO_STREAM("Loaded " << name << ": " << ans);
    } else {
        ROS_ERROR_STREAM("Failed to load " << name);
    }
    return ans;
}

template<typename T>
T RosUtils::readParam(ros::NodeHandle &n, std::string name, T default_value) {
    T ans;
    name = ros::this_node::getName() + "/" + name;
    if (n.getParam(name, ans)) {
        ROS_INFO_STREAM("Loaded " << name);
    } else {
        ROS_ERROR_STREAM("Failed to load " << name << " Use default !");
        return default_value;
    }
    return ans;
}

#endif //SRC_ROSUTILS_H
